/********************************************
 *    卡尔曼滤波公式
 *
 *
 * Created by zlj on 2024/9/10.
********************************************/
#include "kalman.h"

// 初始化卡尔曼滤波器
void kalman_init(KalmanFilter *kf, float process_noise, float measurement_noise, float estimate_error, float initial_value) {
    kf->Q = process_noise;  // 设置过程噪声
    kf->R = measurement_noise;  // 设置测量噪声
    kf->P = estimate_error;  // 设置初始估计误差
    kf->x = initial_value;   // 设置初始状态
}

// 卡尔曼滤波更新步骤
float kalman_update(KalmanFilter *kf, float measurement) {
    // 预测步骤（状态预测已经在上次迭代完成，这里只预测误差协方差）
    kf->P = kf->P + kf->Q;

    // 计算卡尔曼增益
    kf->K = kf->P / (kf->P + kf->R);

    // 更新步骤
    kf->x = kf->x + kf->K * (measurement - kf->x);  // 更新状态估计
    kf->P = (1 - kf->K) * kf->P;  // 更新误差协方差

    return kf->x;  // 返回估计的状态
}